#include <stdio.h>
#include <math.h>
#include <iostream>
#include <libplayerc++/playerc++.h>
#include "defs.h"
#include <sys/ipc.h>
#include <sys/types.h>
#include <sys/shm.h>

void converte_matocup_matcus(int matriz_ocupacao[TAMANHO_OCUPACAO][TAMANHO_OCUPACAO], int matriz_custos[TAMANHO_CUSTOS][TAMANHO_CUSTOS])
{
  int i, j;
  
  for (i = 0; i < TAMANHO_CUSTOS; i++)
    for (j = 0; j < TAMANHO_CUSTOS; j++)  
	if (matriz_ocupacao[i/TAMANHO_OCUPACAO][i%TAMANHO_OCUPACAO] == -1 && matriz_ocupacao[j/TAMANHO_OCUPACAO][j%TAMANHO_OCUPACAO] == -1 && ((i == (j + 1) && (i % TAMANHO_OCUPACAO != 0)) || (i == (j - 1) && (i % TAMANHO_OCUPACAO != (TAMANHO_OCUPACAO - 1))) || (i == (j + TAMANHO_OCUPACAO)) || (i == (j - TAMANHO_OCUPACAO))))
	{
	   matriz_custos[i][j] = 1;
	}
	else
	  matriz_custos[i][j] = -1;
}

int main(int argc, char** argv)
{
	using namespace PlayerCc;

	int shmid;
	char* shm, *s;
	key_t key = KEY_SHDMEM;

	// Cria area de memoria compartilhada (create & read-write)
	if ((shmid = shmget(key, sizeof(int)*TAMANHO_CUSTOS*TAMANHO_CUSTOS, 0666)) == -1)
	{
   		perror("shmget"); 
		exit(-1);
	}

	// Anexa o processo corrente a area recem criada
	if ((shm = (char*) shmat(shmid, NULL, 0)) == (char *) -1) 
	{
        	perror("shmat");
	        exit(-1);
	}

	PlayerClient robot("localhost");
	Position2dProxy pp(&robot,0);
	MapProxy mp(&robot,0);

	int i, j, numero_vertices, quadrados_restantes;
	int matriz_ocupacao[TAMANHO_OCUPACAO][TAMANHO_OCUPACAO];
	int matriz_custos[TAMANHO_CUSTOS][TAMANHO_CUSTOS];
	int *matriz_custos_linearizada;

	//Calibra os valores das coordenadas do Robo
	matriz_custos_linearizada = (int *)malloc (sizeof(int)*(TAMANHO_CUSTOS*TAMANHO_CUSTOS));
  
	mp.RequestMap();

	uint h = mp.GetHeight();
	uint wm = mp.GetWidth();

	int8_t* map;
	map = (int8_t*) malloc(sizeof(int8_t) * wm * h);
	mp.GetMap(map);

	int8_t im, jm;
	for (i = 0; i < TAMANHO_OCUPACAO; i++)
		for (j = 0; j < TAMANHO_OCUPACAO; j++)
			if ((i == 1 && j == 1) || (i == 1 && j == 2) || (i == 2 && j == 1) || (i == 2 && j == 2) || (i == 3 && j == 1) || (i == 3 && j == 2))
				matriz_ocupacao[i][j] = 1;
			else
				matriz_ocupacao[i][j] = -1;

	converte_matocup_matcus(matriz_ocupacao, matriz_custos);

	s = shm;
	for (i = 0; i < TAMANHO_CUSTOS; i++)
	{
		for (j = 0; j <  TAMANHO_CUSTOS; j++)
		{
			matriz_custos_linearizada[i * TAMANHO_CUSTOS + j] = matriz_custos[i][j];
			*s = matriz_custos[i][j];
			s++;
		}
	}
}


